/******************** (C) COPYRIGHT 2006 STMicroelectronics ********************
* File Name          : 91x_init.s
* Author             : MCD Application Team
* Date First Issued  : 05/18/2006 : Version 1.0
* Description        : This module performs:
*                      - FLASH/RAM initialization,
*                      - Stack pointer initialization for each mode ,
*                      - Branches to ?main in the C library (which eventually
*                        calls main()).
*
*          On reset, the ARM core starts up in Supervisor (SVC) mode,
*          in ARM state,with IRQ and FIQ disabled.
*******************************************************************************/


// --- Standard definitions of mode bits and interrupt (I & F) flags in PSRs

.set Mode_USR, 0x10
.set Mode_FIQ, 0x11
.set Mode_IRQ, 0x12
.set Mode_SVC, 0x13
.set Mode_ABT, 0x17
.set Mode_UND, 0x1B
.set Mode_SYS, 0x1F

.set I_Bit, 0x80
.set F_Bit, 0x40

//---------------------------------------
//--- BASE ADDRESSES
// System memory locations
// Stacks.
//---------------------------------------
        .extern __stack_end__

// NOTE:
// Tasks run in system mode but task stacks are allocated when the task is created.



        .include "stack_sizes.inc"

.set SVC_Stack_Offset, 0
.set IRQ_Stack_Offset, SVC_Stack_Offset+SVC_Stack_Size
.set USR_Stack_Offset, IRQ_Stack_Offset+IRQ_Stack_Size
.set FIQ_Stack_Offset, USR_Stack_Offset+USR_Stack_Size
.set ABT_Stack_Offset, FIQ_Stack_Offset+FIQ_Stack_Size
.set UND_Stack_Offset, ABT_Stack_Offset+ABT_Stack_Size
.set ALL_STACK_SIZE,   (SVC_Stack_Size+IRQ_Stack_Size+USR_Stack_Size+FIQ_Stack_Size+ABT_Stack_Size)/4


// STR9X register specific definition
.set FMI_BBSR_AHB_UB,      0x54000000
.set FMI_BBADR_AHB_UB,     0x5400000C
.set FMI_NBBSR_AHB_UB,     0x54000004
.set FMI_NBBADR_AHB_UB,    0x54000010

.set  SCU_SCRO_APB1_UB,    0x4C002034
.set  SCRO_AHB_UNB,        0x5C002034



//---------------------------------------------------------------
// program_start
//---------------------------------------------------------------

.section .resethnd,"ax"
	.code 32
	.align  0

        .globl    __program_start
        .extern   main

__program_start:

        NOP   // execute some instructions to access CPU registers after wake
        NOP   // up from Reset, while waiting for OSC stabilization
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP


// --- Remap Flash Bank 0 at address 0x0 and Bank 1 at address 0x80000,
//     when the bank 0 is the boot bank, then enable the Bank 1.

        LDR R6, =0x54000000
        LDR R7, =0x4
        STR R7, [R6]

        LDR R6, =0x54000004
        LDR R7, =0x3
        STR R7, [R6]

        LDR R6, =0x5400000C
        LDR R7, =0x0
        STR R7, [R6]

        LDR R6, =0x54000010
        LDR R7, =0x20000
        STR R7, [R6]

        LDR R6, =0x54000018
        LDR R7, =0x18
        STR R7, [R6]

// --- Enable 96K RAM
        LDR     R0, = SCRO_AHB_UNB
        LDR     R1, = 0x0196
        STR     R1, [R0]


//----------------------------------------------------------------------------
//    Setup a stack for each mode - note that this only sets up a usable stack
//    for system/user, SWI and IRQ modes.   Also each mode is setup with
//    interrupts initially disabled.
//    Piotr: Aborts and Undefinded uses the same stack since they are critical
//           exceptions and there is no need for separete stacks
//----------------------------------------------------------------------------

   MSR     CPSR_c, #Mode_FIQ|I_Bit|F_Bit   // No interrupts
   LDR      SP, =__stack_end__ - FIQ_Stack_Offset

   MSR     CPSR_c, #Mode_IRQ|I_Bit|F_Bit   // No interrupts
   LDR      SP, =__stack_end__ - IRQ_Stack_Offset

   MSR     CPSR_c, #Mode_ABT|I_Bit|F_Bit   // No interrupts
   LDR      SP, =__stack_end__ - ABT_Stack_Offset

   MSR     CPSR_c, #Mode_UND|I_Bit|F_Bit   // No interrupts
   LDR      SP, =__stack_end__ - ABT_Stack_Offset

   MSR     CPSR_c, #Mode_SVC|I_Bit|F_Bit   // No interrupts
   LDR      SP, =__stack_end__ - SVC_Stack_Offset

   MSR      CPSR_c, #Mode_SYS|I_Bit|F_Bit  // No interrupts
   LDR      SP, =__stack_end__ - USR_Stack_Offset

/* We want to start in supervisor mode.  Operation will switch to system
   mode when the first task starts. */
   MSR   CPSR_c, #Mode_SVC|I_Bit|F_Bit


// --- Set bits 17-18 of the Core Configuration Control Register

   MOV     r0, #0x60000
   MCR     p15,0x1,r0,c15,c1,0


/* Copy the data segment initializers from flash to SRAM */
  movs  r1, #0
  b  LoopCopyDataInit

CopyDataInit:
  ldr  r3, =__data_beg_src__
  ldr  r3, [r3, r1]
  str  r3, [r0, r1]
  adds  r1, r1, #4

LoopCopyDataInit:
  ldr  r0, =__data_beg__
  ldr  r3, =__data_end__
  adds  r2, r0, r1
  cmp  r2, r3
  bcc  CopyDataInit
  ldr  r2, =__bss_beg__
  b  LoopFillZerobss
/* Zero fill the bss segment. */
FillZerobss:
  movs  r3, #0
  str  r3, [r2], #4
LoopFillZerobss:
  ldr  r3, = __bss_end__
  cmp  r2, r3
  bcc  FillZerobss


/*Fill Stack Areaa with "magic" values*/
  ldr  r2, =__variables_end__
  b  LoopFillSysStackPattern
FillSysStackPattern:
  ldr  r3, =0xBEEFBEEF
  str  r3, [r2], #4
LoopFillSysStackPattern:
  ldr  r3, = __stack_end__
  cmp  r2, r3
  bcc  FillSysStackPattern


// --- Now enter the C code
   B        a_main   // Note : use B not BL, because an application will
                   // never return this way

